#include <stdio.h>
#include <math.h>
#include <conio.h>
#include "defs.h"
#include "utils.h"
#include "setup.h"
#include "kernel.h"
#include "gcode.h"
#include "control.h"
#include "operator.h"

extern int version;

int do_gcode(struct hard_setup *hardware, struct soft_setup *software, struct gcode_setup *gcode,
             struct control *ctrl, struct pos *world, char ch, double num)
{
   static double x_now, y_now, z_now, a_now, angle_now;
   static double X, Y, Z, A, I, J, K, R, Q, dcycleZ, dcycleR, dcycleQ, factor=1;
   static float fd_now, cur_sp;
   static int ENT_TYPE=LINE_ENT, ent_type_now=LINE_ENT, par;
   static int arc=XY_ARC_ENT, rot=CW, dwell, fd_ch, set_coord;
   static int new_X, new_Y, new_Z, new_A, new_I, new_J, new_K, new_R, new_Q;
   static int new_tool, init_M00, init_M30=ON, new_scale;
   static int dcycle81, dcycle83, dcycle_init, ent_holder, drilling;
   static int line_by_line;
   static BYTE buffer[1000];
   static struct pos pt_now, world_now, scale;
   struct pos pt, begin;
   double x, y, z, a, xe, ye, ze, dx, dy, dz, da, angle, radius, ms, dtemp;
   float fast_speed;
   int msg=COMPLETE, ent_type, temp;
   
   if(init_M30==ON)
      {dcycleR=dcycleZ=dcycleQ=scale.x_pos=scale.y_pos=scale.z_pos=scale.a_pos=0;
       factor=1;
       dcycle81=dcycle83=new_tool=dwell=new_scale=OFF;
       ENT_TYPE=ent_type_now=LINE_ENT; par=FALSE;
       arc=XY_ARC_ENT; rot=CW;
      }
   if(init_M00==ON || init_M30==ON)
      {init_M00=init_M30=OFF;
       x_now=y_now=z_now=a_now=angle_now=0; fd_ch=TRUE;
      }
   if(ch==1) line_by_line=ON;
   if(ch==2) line_by_line=OFF;

   if(ch=='\n' && par==TRUE)
     {MOVE:
      par=FALSE;
      a=get_move_val(&A,&(*world).a_pos,&(*gcode).a_coord[(*ctrl).cs],&scale.a_pos,&factor,&(*ctrl).pty)*new_A;
      if(ENT_TYPE==LINE_ENT || ENT_TYPE==LINE_G0_ENT)
        {x=get_move_val(&X,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_X;
         y=get_move_val(&Y,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_Y;
         z=get_move_val(&Z,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_Z;
         angle=NULL; ent_type=ENT_TYPE;}
      if(ENT_TYPE==XY_ARC_ENT)
        {xe=get_move_val(&X,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_X;
         ye=get_move_val(&Y,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_Y;
         if((*gcode).relarc==ON) {temp=(*ctrl).pty; (*ctrl).pty=REL;}
         x=get_move_val(&I,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_I;
         y=get_move_val(&J,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_J;
         if((*gcode).relarc==ON) (*ctrl).pty=temp;
         z=get_move_val(&Z,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_Z;
         angle=get_ang(rot,-x,-y,xe-x,ye-y); ent_type=ENT_TYPE;}
      if(ENT_TYPE==YZ_ARC_ENT)
        {ye=get_move_val(&Y,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_Y;
         ze=get_move_val(&Z,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_Z;
         if((*gcode).relarc==ON) {temp=(*ctrl).pty; (*ctrl).pty=REL;}
         y=get_move_val(&I,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_J;
         z=get_move_val(&K,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_K;
         if((*gcode).relarc==ON) (*ctrl).pty=temp;
         x=get_move_val(&X,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_X;
         angle=get_ang(rot,-y,-z,ye-y,ze-z); ent_type=ENT_TYPE;}
      if(ENT_TYPE==ZX_ARC_ENT)
        {ze=get_move_val(&Z,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_Z;
         xe=get_move_val(&X,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_X;
         if((*gcode).relarc==ON) {temp=(*ctrl).pty; (*ctrl).pty=REL;}
         x=get_move_val(&I,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_I;
         z=get_move_val(&K,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_K;
         if((*gcode).relarc==ON) (*ctrl).pty=temp;
         y=get_move_val(&Y,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_Y;
         angle=get_ang(rot,-z,-x,ze-z,xe-x); ent_type=ENT_TYPE;}

      START_MOVE:
      pt.x_pos=((*world).x_pos-(*gcode).x_coord[(*ctrl).cs])*(*ctrl).pty;
      pt.y_pos=((*world).y_pos-(*gcode).y_coord[(*ctrl).cs])*(*ctrl).pty;
      pt.z_pos=((*world).z_pos-(*gcode).z_coord[(*ctrl).cs])*(*ctrl).pty;
      pt.a_pos=((*world).a_pos-(*gcode).a_coord[(*ctrl).cs])*(*ctrl).pty;

      if(dcycle_init==ON) {dcycle_init=OFF;
                           if(new_Z) dcycleZ=Z;
                           if(new_R) dcycleR=R;
                           if(new_Q) dcycleQ=fabs(Q);
                           ch='S';
                           if(dcycle81==ON) num=151;
                           if(dcycle83==ON) num=156;
                           goto DONE;}
      if(set_coord==ON) {pt.x_pos=pt.x_pos-(pt.x_pos*(*ctrl).pty-X)*new_X;
                         pt.y_pos=pt.y_pos-(pt.y_pos*(*ctrl).pty-Y)*new_Y;
                         pt.z_pos=pt.z_pos-(pt.z_pos*(*ctrl).pty-Z)*new_Z;
                         pt.a_pos=pt.a_pos-(pt.a_pos*(*ctrl).pty-A)*new_A;
                         goto DONE;}
      if(new_scale==ON) {new_scale=OFF;
                         scale.x_pos=(X+((*world).x_pos-(*gcode).x_coord[(*ctrl).cs])-
                                     ((*world).x_pos-(*gcode).x_coord[(*ctrl).cs])*(*ctrl).pty)*new_X;
                         scale.y_pos=(Y+((*world).y_pos-(*gcode).y_coord[(*ctrl).cs])-
                                     ((*world).y_pos-(*gcode).y_coord[(*ctrl).cs])*(*ctrl).pty)*new_Y;
                         scale.z_pos=(Z+((*world).z_pos-(*gcode).z_coord[(*ctrl).cs])-
                                     ((*world).z_pos-(*gcode).z_coord[(*ctrl).cs])*(*ctrl).pty)*new_Z;
                         scale.a_pos=(A+((*world).a_pos-(*gcode).a_coord[(*ctrl).cs])-
                                     ((*world).a_pos-(*gcode).a_coord[(*ctrl).cs])*(*ctrl).pty)*new_A;
                         goto DONE;}

      if((*ctrl).cvv==OFF || line_by_line==ON || dcycle81==ON || dcycle83==ON)
        {begin=pt;

         CONTINUE_PTP_MOVE:
         if(ent_type==LINE_G0_ENT && (*gcode).safer==ON)
           {fast_speed=HIGH_SPEED;
            msg=move(hardware, software, &pt, &cur_sp, &fast_speed, x, y, z, a, angle, LINE_ENT,
                     0, 0, 0, 0, NULL, LINE_ENT);
           }
         else
           {msg=move(hardware, software, &pt, &cur_sp, &(*ctrl).frate, x, y, z, a, angle, ent_type,
                     0, 0, 0, 0, NULL, LINE_ENT);
           }
         if((msg==PAUSE || msg==BREAK || msg==E_STOP || (msg>=X_PLIMIT && msg<=A_NLIMIT)) && drilling==ON)
           {drilling=OFF; ENT_TYPE=ent_holder;}
         if(msg==BREAK)
           {msg=break_msg(hardware, software, (*gcode).rp[(*ctrl).cs], (*gcode).a_rp[(*ctrl).cs],
                          &begin, &pt, &x, &y, &z, &a, &angle, ent_type);
            if(msg==COMPLETE) goto CONTINUE_PTP_MOVE; if(msg==BREAK) {ch='S'; num=100;}
           }
         if(msg==E_STOP) init_M00=ON;
         if(msg>=X_PLIMIT && msg<=A_NLIMIT) init_M00=ON;
         if(dcycle81==ON && ch!='S') {ch='S'; num=151;}
         if(dcycle83==ON && ch!='S') {ch='S'; num=156;}
        }

      else  //cvv move
        {begin=pt_now;

         CONTINUE_CVV_MOVE:
         if(ent_type_now==LINE_G0_ENT && (*gcode).safer==ON)
           {fast_speed=HIGH_SPEED;
            msg=move(hardware, software, &pt_now, &cur_sp, &fast_speed, x_now, y_now, z_now, a_now,
                     angle_now, LINE_ENT, x, y, z, a, angle, LINE_ENT);
           }
         else
           {msg=move(hardware, software, &pt_now, &cur_sp, &fd_now, x_now, y_now, z_now, a_now,
                     angle_now, ent_type_now, x, y, z, a, angle, ent_type);
           }

         if(msg==BREAK)
           {msg=break_msg(hardware, software, (*gcode).rp[(*ctrl).cs], (*gcode).a_rp[(*ctrl).cs],
                          &begin, &pt_now, &x_now, &y_now, &z_now, &a_now, &angle_now, ent_type_now);
            if(msg==COMPLETE) goto CONTINUE_CVV_MOVE; if(msg==BREAK) {msg=M0; ch='S'; num=100; pt=pt_now; goto DONE;}
           }
         if(msg==E_STOP) {init_M00=ON; pt=pt_now; goto DONE;}
         if(msg>=X_PLIMIT && msg<=A_NLIMIT) {init_M00=ON; pt=pt_now; goto DONE;}

         pt_now=pt; x_now=x; y_now=y; z_now=z; a_now=a; angle_now=angle; ent_type_now=ent_type;
         world_now=*world;
         if(fd_ch==TRUE) {fd_ch=FALSE; fd_now=(*ctrl).frate; update_display(software, 0, 0, 0, 0, 0, 0, (*ctrl).frate, 6);}
         else (*ctrl).frate=fd_now;

         if(ent_type_now==LINE_ENT || ent_type_now==LINE_G0_ENT)
           {dx=x; dy=y; dz=z;}
         if(ent_type_now==XY_ARC_ENT)
           {radius=sqrt(x*x+y*y); dx=x+radius*cos(ang_of(-y,-x)+angle); dy=y+radius*sin(ang_of(-y,-x)+angle); dz=z;}
         if(ent_type_now==YZ_ARC_ENT)
           {radius=sqrt(y*y+z*z); dy=y+radius*cos(ang_of(-z,-y)+angle); dz=z+radius*sin(ang_of(-z,-y)+angle); dx=x;}
         if(ent_type_now==ZX_ARC_ENT)
           {radius=sqrt(z*z+x*x); dz=z+radius*cos(ang_of(-x,-z)+angle); dx=x+radius*sin(ang_of(-x,-z)+angle); dy=y;}
         if(a==0 && (*software).a_follow!=INDEPENDANT)
           {if((*software).a_follow==FOLLOW_X) da=dx; if((*software).a_follow==FOLLOW_Y) da=dy; if((*software).a_follow==FOLLOW_Z) da=dz;}
         else da=a;
         pt.x_pos=pt_now.x_pos+dx; pt.y_pos=pt_now.y_pos+dy; pt.z_pos=pt_now.z_pos+dz; pt.a_pos=pt_now.a_pos+da;
        }
      if(msg==PAUSE) {ch='M'; num=0;}

      DONE:
      (*world).x_pos=(*world).x_pos+pt.x_pos+((*gcode).x_coord[(*ctrl).cs]-(*world).x_pos)*(*ctrl).pty;
      (*world).y_pos=(*world).y_pos+pt.y_pos+((*gcode).y_coord[(*ctrl).cs]-(*world).y_pos)*(*ctrl).pty;
      (*world).z_pos=(*world).z_pos+pt.z_pos+((*gcode).z_coord[(*ctrl).cs]-(*world).z_pos)*(*ctrl).pty;
      (*world).a_pos=(*world).a_pos+pt.a_pos+((*gcode).a_coord[(*ctrl).cs]-(*world).a_pos)*(*ctrl).pty;
      if(par==FALSE) {new_X=new_Y=new_Z=new_A=new_I=new_J=new_K=new_R=FALSE;}

      if(set_coord==ON)
        {set_coord=OFF;
         for(temp=0;temp<4;temp++)
           {if(temp==3) temp++;
            update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                           (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, 0, temp);
           }
        }
     }

   if(ch=='X') {new_X=TRUE; X=num; par=TRUE;}
   if(ch=='Y') {new_Y=TRUE; Y=num; par=TRUE;}
   if(ch=='Z') {new_Z=TRUE; Z=num; par=TRUE;}
   if(ch==(*software).a_label && (*software).a_status==ON) {new_A=TRUE; A=num; par=TRUE;}
   if(ch=='I') {new_I=TRUE; I=num; par=TRUE;}
   if(ch=='J') {new_J=TRUE; J=num; par=TRUE;}
   if(ch=='K') {new_K=TRUE; K=num; par=TRUE;}
   if(ch=='R') {new_R=TRUE; R=num; par=TRUE;}
   if(ch=='Q') {new_Q=TRUE; Q=num; par=TRUE;}
   if(ch=='F')
     {if((*software).time==MIN) (*ctrl).frate=num/60;
      else (*ctrl).frate=num;
      if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) fd_ch=TRUE;
      else update_display(software, 0, 0, 0, 0, 0, 0, (*ctrl).frate, 6);
     }
   if(dwell==ON && ch!='P') dwell=OFF;
   if(dwell==ON && ch=='P') {ms=num; ch='S'; num=142;
                             if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                            }

   if(new_scale==ON && (ch=='\n' || line_by_line==ON)) {par=FALSE; new_scale=OFF;}
   if(new_scale==ON && ch=='P') factor=num;

   if(new_tool==ON) 
     {new_tool=OFF;
      if(ch=='P' && num<=(*gcode).tnum_max)
        {(*world).z_pos=(*world).z_pos-((*gcode).tool_len[num]-(*gcode).tool_len[(*gcode).tnum]);
         (*gcode).tnum=num; GMTtext(software, ctrl, gcode); 
        }
     }
   
   if(ch=='G')
     switch((int)num)
       {case 0: ENT_TYPE=LINE_G0_ENT; break;
        case 1: ENT_TYPE=LINE_ENT; break;
        case 2: ENT_TYPE=arc; rot=CW; break;
        case 3: ENT_TYPE=arc; rot=CCW; break;
        case 4: dwell=ON; break;
        case 17:
        case 18:
        case 19: if(arc==XY_ARC_ENT) {I=I-(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty; J=J-(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(arc==YZ_ARC_ENT) {I=I-(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty; J=J-(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(arc==ZX_ARC_ENT) {I=I-(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty; J=J-(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(num==17) {arc=XY_ARC_ENT; I=I+(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty; J=J+(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(num==18) {arc=ZX_ARC_ENT; I=I+(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty; J=J+(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(num==19) {arc=YZ_ARC_ENT; I=I+(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty; J=J+(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty;}
                 break;
        case 74:
        case 20: if((*software).units==IN) break;
                 ch='S'; num=140;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 75:
        case 21: if((*software).units==MM) break;
                 ch='S'; num=141;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 43: new_tool=ON; break;
        case 49: (*world).z_pos=(*world).z_pos+(*gcode).tool_len[(*gcode).tnum];
                 (*gcode).tnum=0; GMTtext(software, ctrl, gcode);
                 break;
        case 50: factor=1; break;
        case 51: new_scale=ON; break;
        case 53:
        case 54:
        case 55:
        case 56:
        case 57:
        case 58:
        case 59: X=X+(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty; Y=Y+(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty;
                 Z=Z+(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty; A=A+(*gcode).a_coord[(*ctrl).cs]*(*ctrl).pty;
                 if(arc==XY_ARC_ENT) {I=I+(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty; J=J+(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(arc==YZ_ARC_ENT) {I=I+(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty; J=J+(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(arc==ZX_ARC_ENT) {I=I+(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty; J=J+(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty;}
                 (*ctrl).cs=num-53;
                 X=X-(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty; Y=Y-(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty;
                 Z=Z-(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty; A=A-(*gcode).a_coord[(*ctrl).cs]*(*ctrl).pty;
                 if(arc==XY_ARC_ENT) {I=I-(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty; J=J-(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(arc==YZ_ARC_ENT) {I=I-(*gcode).y_coord[(*ctrl).cs]*(*ctrl).pty; J=J-(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty;}
                 if(arc==ZX_ARC_ENT) {I=I-(*gcode).z_coord[(*ctrl).cs]*(*ctrl).pty; J=J-(*gcode).x_coord[(*ctrl).cs]*(*ctrl).pty;}
                 GMTtext(software, ctrl, gcode); break;
        case 60:
                 if((*ctrl).cvv==OFF) break;
                 (*ctrl).cvv=OFF; GMTtext(software, ctrl, gcode);
                 if(dcycle81==ON || dcycle83==ON || line_by_line==ON) break;
                 x=x_now; y=y_now; z=z_now; a=a_now; angle=angle_now; ent_type=ent_type_now; *world=world_now;
                 x_now=y_now=z_now=a_now=angle_now=0;
                 ent_type_now=LINE_ENT; fd_ch=TRUE;
                 ch='*'; goto START_MOVE;
        case 64: 
                 if((*ctrl).cvv==ON) break;
                 (*ctrl).cvv=ON; GMTtext(software, ctrl, gcode);
                 break;
        case 70: if((*software).a_follow==INDEPENDANT) break;
                 ch='S'; num=130;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 71: if((*software).a_follow==FOLLOW_X) break;
                 ch='S'; num=131;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 72: if((*software).a_follow==FOLLOW_Y) break;
                 ch='S'; num=132;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 73: if((*software).a_follow==FOLLOW_Z) break;
                 ch='S'; num=133;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 80: 
                 if(dcycle81==OFF && dcycle83==OFF) break;
                 dcycle81=dcycle83=OFF;
                 break;
        case 81: 
                 if(dcycle81==ON) break;
                 if(dcycle83==ON) dcycle83=OFF;
                 ch='S'; num=150;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 83: 
                 if(dcycle83==ON) break;
                 if(dcycle81==ON) dcycle81=OFF;
                 ch='S'; num=155;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 90: (*ctrl).pty=ABS;
                 X=(*world).x_pos-(*gcode).x_coord[(*ctrl).cs];
                 Y=(*world).y_pos-(*gcode).y_coord[(*ctrl).cs];
                 Z=(*world).z_pos-(*gcode).z_coord[(*ctrl).cs];
                 A=(*world).a_pos-(*gcode).a_coord[(*ctrl).cs];
                 if(arc==XY_ARC_ENT) {I=(*world).x_pos-(*gcode).x_coord[(*ctrl).cs]; J=(*world).y_pos-(*gcode).y_coord[(*ctrl).cs];}
                 if(arc==YZ_ARC_ENT) {I=(*world).y_pos-(*gcode).y_coord[(*ctrl).cs]; J=(*world).z_pos-(*gcode).z_coord[(*ctrl).cs];}
                 if(arc==ZX_ARC_ENT) {I=(*world).z_pos-(*gcode).z_coord[(*ctrl).cs]; J=(*world).x_pos-(*gcode).x_coord[(*ctrl).cs];}
                 GMTtext(software, ctrl, gcode); break;
        case 91: (*ctrl).pty=REL; X=Y=Z=A=I=J=0; GMTtext(software, ctrl, gcode); break;
        case 92: if((*ctrl).cs==0) {par=TRUE; set_coord=ON;} break;
        default: msg=INVALID_CODE; break;
       }
   if(ch=='M')
     switch((int)num)
       {case 3: if((*ctrl).M03==ON) break;
                ch='S'; num=120;
                if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                break;
        case 5: if((*ctrl).M03==OFF) break;
                ch='S'; num=121;
                if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                break;
        case 7: if((*ctrl).M07==ON) break;
                ch='S'; num=122;
                if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                break;
        case 8: if((*ctrl).M08==ON) break;
                ch='S'; num=124;
                if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                break;
        case 9: if((*ctrl).M08==OFF && (*ctrl).M07==OFF) break;
                 ch='S'; num=125;
                if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                break;
        case 10: if((*ctrl).M10==ON) break;
                 ch='S'; num=126;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 11: if((*ctrl).M10==OFF) break;
                 ch='S'; num=127;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 6: z=(*gcode).tool_ch_z-(*world).z_pos-(*gcode).tool_len[(*gcode).tnum]; a=(*gcode).tool_ch_a-(*world).a_pos;
                x=y=0; angle=NULL; ent_type=LINE_G0_ENT;
                ch='S'; num=110; goto START_MOVE;
        case 0:
        case 30:
        case 99: ch='S'; if(num==0) num=100; else if(num==30) num=101; else num=102;
                 if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                 break;
        case 100: //rotate +90 about x axis
                  rotate_x(software, hardware, world);
                  update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                                 (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, (*ctrl).frate, ALL);
                  break;
        case 101: //rotate -90 about x axis
                  rotate_x(software, hardware, world); rotate_x(software, hardware, world); rotate_x(software, hardware, world);
                  update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                                 (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, (*ctrl).frate, ALL);
                  break;
        case 102: //rotate +90 about y axis
                  rotate_y(software, hardware, world);
                  update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                                 (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, (*ctrl).frate, ALL);
                  break;
        case 103: //rotate -90 about y axis
                  rotate_y(software, hardware, world); rotate_y(software, hardware, world); rotate_y(software, hardware, world);
                  update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                                 (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, (*ctrl).frate, ALL);
                  break;
        case 104: //rotate +90 about z axis
                  rotate_z(software, hardware, world);
                  update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                                 (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, (*ctrl).frate, ALL);
                  break;
        case 105: //rotate -90 about z axis
                  rotate_z(software, hardware, world); rotate_z(software, hardware, world); rotate_z(software, hardware, world);
                  update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                                 (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, (*ctrl).frate, ALL);
                  break;
        case 106: //switch z and a
                  switch_z_a(software, hardware, world);
                  update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                                 (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, (*ctrl).frate, ALL);
                  break;
        default: msg=INVALID_CODE; break;
       }
   if(ch=='S')
     switch((int)num)
       {case 100: init_M00=ON; msg=M0; break;
        case 101: init_M30=ON; msg=M30; break;
        case 102: init_M30=ON; msg=M99; break;
        case 110: x=(*gcode).tool_ch_x-(*world).x_pos; y=(*gcode).tool_ch_y-(*world).y_pos;
                  a=z=0; angle=NULL; ent_type=LINE_G0_ENT;
                  ch='M'; num=0; goto START_MOVE;
        case 120: if((*hardware).io1_on==HIGH) set_io_bit(hardware, (*hardware).io1_bit);
                  else clear_io_bit(hardware, (*hardware).io1_bit); 
                  (*ctrl).M03=ON; GMTtext(software, ctrl, gcode); break;
        case 121: if((*hardware).io1_on==HIGH) clear_io_bit(hardware, (*hardware).io1_bit);
                  else set_io_bit(hardware, (*hardware).io1_bit); 
                  (*ctrl).M03=OFF; GMTtext(software, ctrl, gcode); break;
        case 122: if((*hardware).io2_on==HIGH) set_io_bit(hardware, (*hardware).io2_bit);
                  else clear_io_bit(hardware, (*hardware).io2_bit); 
                  (*ctrl).M07=ON; GMTtext(software, ctrl, gcode); break;
        case 124: if((*hardware).io3_on==HIGH) set_io_bit(hardware, (*hardware).io3_bit);
                  else clear_io_bit(hardware, (*hardware).io3_bit); 
                  (*ctrl).M08=ON; GMTtext(software, ctrl, gcode); break;
        case 125: if((*hardware).io2_on==HIGH) clear_io_bit(hardware, (*hardware).io2_bit);
                  else set_io_bit(hardware, (*hardware).io2_bit);
                  if((*hardware).io3_on==HIGH) clear_io_bit(hardware, (*hardware).io3_bit);
                  else set_io_bit(hardware, (*hardware).io3_bit); 
                  (*ctrl).M07=(*ctrl).M08=OFF; GMTtext(software, ctrl, gcode); break;
        case 126: if((*hardware).io4_on==HIGH) set_io_bit(hardware, (*hardware).io4_bit);
                  else clear_io_bit(hardware, (*hardware).io4_bit);
                  (*ctrl).M10=ON; GMTtext(software, ctrl, gcode); break;
        case 127: if((*hardware).io4_on==HIGH) clear_io_bit(hardware, (*hardware).io4_bit);
                  else set_io_bit(hardware, (*hardware).io4_bit); 
                  (*ctrl).M10=OFF; GMTtext(software, ctrl, gcode); break;
        case 130: (*software).a_follow=INDEPENDANT; break;
        case 131: (*software).a_follow=FOLLOW_X; break;
        case 132: (*software).a_follow=FOLLOW_Y; break;
        case 133: (*software).a_follow=FOLLOW_Z; break;
        case 140: X=X/25.4; Y=Y/25.4; Z=Z/25.4; A=change_aux_unit(software, A, IN); I=I/25.4; J=J/25.4;
                  cur_sp=cur_sp/25.4; (*ctrl).frate=(*ctrl).frate/25.4;
                  change_pt_units(software, world, IN, IN);
                  change_gcode_units(software, gcode, IN, IN);
                  change_units(hardware, software, IN, IN);
                  GMTtext(software, ctrl, gcode); break;
        case 141: X=X*25.4; Y=Y*25.4; Z=Z*25.4; A=change_aux_unit(software, A, MM); I=I*25.4; J=J*25.4;
                  cur_sp=cur_sp*25.4; (*ctrl).frate=(*ctrl).frate*25.4;
                  change_pt_units(software, world, MM, MM);
                  change_gcode_units(software, gcode, MM, MM);
                  change_units(hardware, software, MM, MM);
                  GMTtext(software, ctrl, gcode); break;
        case 142: update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                                 (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, (*ctrl).frate, ALL);
                  dwell=OFF;
                  if((temp=pause(hardware, software, ms/1000, &(*ctrl).frate))==BREAK)
                    {gettext(22,19,59,24,buffer);
                     box(22,19,59,24, RED,BLACK,WHITE,WHITE);
                       {gotoxy(4,3); cprintf("             Abort?");
                        gotoxy(4,4); cprintf("   <Y> Yes              <N> No");
                       }
                     window(1,1,80,25);
                     while(1)
                       {ch=getch();
                        if(ch=='y' || ch=='Y' || ch=='N' || ch=='n') break;
                       }
                     puttext(22,19,59,24,buffer);
                     if(ch=='y' || ch=='y') {init_M00=ON; msg=M0;}
                    }
                  if(temp==PAUSE) {init_M00=ON; msg=M0;}
                  else if(temp==E_STOP) {init_M00=ON; msg=E_STOP;}
                  break;
        case 150: dcycle81=ON; dcycle_init=ON; par=TRUE;
                  if((*ctrl).cvv==ON) fd_ch=TRUE;
                  break;
        case 151: drilling=ON; ent_holder=ENT_TYPE; ENT_TYPE=LINE_G0_ENT;
                  if((*ctrl).pty==REL) Z=dcycleR-((*world).z_pos-(*gcode).z_coord[(*ctrl).cs]);
                  else Z=dcycleR;
                  new_Z=TRUE; num=152; goto MOVE;
        case 152: ENT_TYPE=LINE_ENT;
                  if((*ctrl).pty==REL) Z=dcycleZ-((*world).z_pos-(*gcode).z_coord[(*ctrl).cs]);
                  else Z=dcycleZ;
                  new_Z=TRUE; num=153; goto MOVE;
        case 153: ENT_TYPE=LINE_G0_ENT;
                  if((*ctrl).pty==REL) Z=dcycleR-((*world).z_pos-(*gcode).z_coord[(*ctrl).cs]);
                  else Z=dcycleR;
                  new_Z=TRUE; num=154; goto MOVE;
        case 154: ENT_TYPE=ent_holder; drilling=OFF;
                  break;
        case 155: dcycle83=ON; dcycle_init=ON; par=TRUE;
                  if((*ctrl).cvv==ON) fd_ch=TRUE;
                  break;
        case 156: drilling=ON; ent_holder=ENT_TYPE; ENT_TYPE=LINE_G0_ENT;
                  if((*ctrl).pty==REL) Z=dcycleR-((*world).z_pos-(*gcode).z_coord[(*ctrl).cs]);
                  else Z=dcycleR;
                  dtemp=dcycleR; new_Z=TRUE; num=157; goto MOVE;
        case 157: ENT_TYPE=LINE_ENT;
                  if(fabs(dcycleZ-dtemp)>dcycleQ)
                    {if(dcycleZ-dtemp<0) dtemp=dtemp-dcycleQ;
                     else dtemp=dtemp+dcycleQ;
                     num=158;}
                  else {dtemp=dcycleZ; num=153;}
                  if((*ctrl).pty==REL) Z=dtemp-((*world).z_pos-(*gcode).z_coord[(*ctrl).cs]);
                  else Z=dtemp;
                  new_Z=TRUE; goto MOVE;
        case 158: ENT_TYPE=LINE_G0_ENT;
                  if((*ctrl).pty==REL) Z=dcycleR-((*world).z_pos-(*gcode).z_coord[(*ctrl).cs]);
                  else Z=dcycleR;
                  new_Z=TRUE; num=159; goto MOVE;
        case 159: ENT_TYPE=LINE_G0_ENT;
                  if((*ctrl).pty==REL) Z=dtemp-((*world).z_pos-(*gcode).z_coord[(*ctrl).cs]);
                  else Z=dtemp;
                  new_Z=TRUE; num=157; goto MOVE;

        default: break;
       }
   return msg;
}

double get_move_val(double *input, double *world, float *cs, double *scale, double *factor, int *pty)
{
   if(*pty==ABS) return ((*input-*scale)**factor+*scale)-(*world-*cs);
   else return *input**factor;
}

double get_ang(int rot, double x1, double y1, double x2, double y2)
{
   double ang1, ang2, ang;
   int s;
   if((x1==0 && y1==0) || (x2==0 && y2==0)) return 0;
   ang1=atan2(y1,x1); ang2=atan2(y2,x2);
   ang=(ang2-ang1)*rot; if(ang<0) s=-1; else s=1;
   if(fabs(s*ang-2*PI)<.00000001 || s*ang<.00000001) ang=0; 
   if(ang<=0) return rot*(ang+2*PI);
   else return rot*ang;
}

double ang_of(double y, double x)
{
   if(y==0 && x==0) return 0;
   return atan2(y,x);
}

int break_msg(struct hard_setup *hardware, struct soft_setup *software, float rp, float a_rp,
              struct pos *begin, struct pos *pt, 
              double *x, double *y, double *z, double *a, double *angle, int ent_type)
{
   double break_ang=0, break_x, break_y, break_z, break_a;
   float sp_limit=0, cur_speed=0, inc=0;
   int msg=COMPLETE, msg2=0, ch, type=SLOW, s, temp1;
   static BYTE buffer[1000], buffer2[1000];
   struct pos stop=*pt;
   
   if(*angle<0) s=-1; else s=1;
   if(ent_type==XY_ARC_ENT)
      break_ang=*angle-get_ang(s, -*x, -*y, -(*begin).x_pos-*x+(*pt).x_pos, -(*begin).y_pos-*y+(*pt).y_pos);
   if(ent_type==YZ_ARC_ENT)
      break_ang=*angle-get_ang(s, -*y, -*z, -(*begin).y_pos-*y+(*pt).y_pos, -(*begin).z_pos-*z+(*pt).z_pos);
   if(ent_type==ZX_ARC_ENT)
      break_ang=*angle-get_ang(s, -*z, -*x, -(*begin).z_pos-*z+(*pt).z_pos, -(*begin).x_pos-*x+(*pt).x_pos);
   break_x=*x-((*pt).x_pos-(*begin).x_pos); break_y=*y-((*pt).y_pos-(*begin).y_pos);
   break_z=*z-((*pt).z_pos-(*begin).z_pos); break_a=*a-((*pt).a_pos-(*begin).a_pos);

   gettext(22,19,59,24,buffer);
   box(22,19,59,24, RED,BLACK,WHITE,WHITE);
   gotoxy(4,1); cprintf("<<Machine Break>> <Esc>To Abort");
   gotoxy(4,3); cprintf("'B'->Continue From Break Point");
   gotoxy(4,4); cprintf("'S'->Continue From Entity Start");
   gotoxy(10,6); cprintf("Jog(Slow) Inc(%s)",prn_val(0));
   for(temp1=0;temp1<6;temp1++) update_display(software, (*pt).x_pos, (*pt).y_pos, (*pt).z_pos, (*pt).a_pos, 0, 0, 0, temp1);
   while((ch=getch())!=27)
     {if(ch==0) ch=getch()+1000;
      switch(ch)
        {case 'b':
         case 'B':
            if(stop.x_pos-(*pt).x_pos!=0 || stop.y_pos-(*pt).y_pos!=0)
              {sp_limit=(*hardware).min_vel_z;
               msg=move(hardware,software, pt, &cur_speed, &sp_limit,
                        0, 0, rp-(*pt).z_pos, a_rp-(*pt).a_pos, NULL, LINE_ENT,
                        0, 0, 0, 0, NULL, LINE_ENT);
               if(msg!=COMPLETE) break;
              }
            sp_limit=HIGH_SPEED;
            msg=move(hardware, software, pt, &cur_speed, &sp_limit,
                     stop.x_pos-(*pt).x_pos, stop.y_pos-(*pt).y_pos, 0, 0, NULL, LINE_ENT,
                     0, 0, 0, 0, NULL, LINE_ENT);
            if(msg!=COMPLETE) break;
            sp_limit=(*hardware).min_vel_z;
            msg=move(hardware, software, pt, &cur_speed, &sp_limit,
                     0, 0, stop.z_pos-(*pt).z_pos, stop.a_pos-(*pt).a_pos, NULL, LINE_ENT,
                     0, 0, 0, 0, NULL, LINE_ENT);
            if(msg!=COMPLETE) break;
            *x=break_x; *y=break_y; *z=break_z; *a=break_a; *angle=break_ang; 
            msg2=CONTINUE;
            break;
         case 's':
         case 'S':
            if((*begin).x_pos-(*pt).x_pos!=0 || (*begin).y_pos-(*pt).y_pos!=0)
              {sp_limit=(*hardware).min_vel_z;
               msg=move(hardware,software, pt, &cur_speed, &sp_limit,
                        0, 0, rp-(*pt).z_pos, a_rp-(*pt).a_pos, NULL, LINE_ENT,
                        0, 0, 0, 0, NULL, LINE_ENT);
               if(msg!=COMPLETE) break;
              }
            sp_limit=HIGH_SPEED;
            msg=move(hardware, software, pt, &cur_speed, &sp_limit,
                     (*begin).x_pos-(*pt).x_pos, (*begin).y_pos-(*pt).y_pos, 0, 0, NULL, LINE_ENT,
                     0, 0, 0, 0, NULL, LINE_ENT);
            if(msg!=COMPLETE) break;
            sp_limit=(*hardware).min_vel_z;
            msg=move(hardware, software, pt, &cur_speed, &sp_limit,
                     0, 0, (*begin).z_pos-(*pt).z_pos, (*begin).a_pos-(*pt).a_pos, NULL, LINE_ENT,
                     0, 0, 0, 0, NULL, LINE_ENT);
            if(msg!=COMPLETE) break;
            msg2=CONTINUE;
            break;
         case 'j':
         case 'J':
            gotoxy(10,6);
            if(type==FAST) {type=SLOW; cprintf("(Slow)");}
            else if(type==SLOW) {type=INC; cprintf("(Inc) ");}
            else if(type==INC) {type=FAST; cprintf("(Fast)");}
            textcolor(WHITE);
            break;
         case 'i':
         case 'I':
            gettext(22,19,59,24,buffer2);
            box(22,19,59,24, RED,BLACK,WHITE,WHITE);
            gotoxy(13,1); cprintf("<Esc> To Exit");
            gotoxy(7,3); cprintf("Enter New Jog Increment:");
            _setcursortype(_NORMALCURSOR);
            gotoxy(16,4); get_value(&inc);
            _setcursortype(_NOCURSOR);
            puttext(22,19,59,24,buffer2);
            gotoxy(25,6); cprintf("(%s)",prn_val(inc));
            break;
         case 72+1000:     //up
         case 77+1000:     //right
            if((ch==72+1000 && (*software).invert_arrows==OFF) || (ch==77+1000 && (*software).invert_arrows==ON))
               msg=jog(hardware, software, pt, type, inc, Y_AXIS, POS);
            else msg=jog(hardware, software, pt, type, inc, X_AXIS, POS);
            break;
         case 80+1000:    //down
         case 75+1000:    //left
            if((ch==80+1000 && (*software).invert_arrows==OFF) || (ch==75+1000 && (*software).invert_arrows==ON))
               msg=jog(hardware, software, pt, type, inc, Y_AXIS, NEG);
            else msg=jog(hardware, software, pt, type, inc, X_AXIS, NEG);
            break;
         case 73+1000:   //page up
            msg=jog(hardware, software, pt, type, inc, Z_AXIS, POS);
            break;
         case 81+1000:   //page down
            msg=jog(hardware, software, pt, type, inc, Z_AXIS, NEG);
            break;
         case '+':
         case '=':
            msg=jog(hardware, software, pt, type, inc, A_AXIS, POS);
            break;
         case '-':
            msg=jog(hardware, software, pt, type, inc, A_AXIS, NEG);
            break;
        }
      for(temp1=0;temp1<6;temp1++) update_display(software, (*pt).x_pos, (*pt).y_pos, (*pt).z_pos, (*pt).a_pos, 0, 0, 0, temp1);
      if((msg>=X_PLIMIT && msg<=A_NLIMIT) || msg==E_STOP || msg==QUIT) break;
      if(msg2==CONTINUE) {msg=COMPLETE; break;}
     }
   if(ch==27) msg=BREAK;
   puttext(22,19,59,24,buffer);
   window(1,1,80,25);
   return msg;
}
